{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "c9GoM16F7I7Z"
   },
   "source": [
    "# Exercises on Rigid Transforms"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "luJIMMUbZpzk"
   },
   "source": [
    "## Notebook Setup \n",
    "The following cell will install Drake, checkout the manipulation repository, and set up the path (only if necessary).\n",
    "- On Google's Colaboratory, this **will take approximately two minutes** on the first time it runs (to provision the machine), but should only need to reinstall once every 12 hours.  \n",
    "\n",
    "More details are available [here](http://manipulation.mit.edu/drake.html)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "UeaGx2uO_O3Q"
   },
   "outputs": [],
   "source": [
    "import importlib\n",
    "import os, sys\n",
    "from urllib.request import urlretrieve\n",
    "\n",
    "if 'google.colab' in sys.modules and importlib.util.find_spec('manipulation') is None:\n",
    "    urlretrieve(f\"http://manipulation.csail.mit.edu/scripts/setup/setup_manipulation_colab.py\",\n",
    "                \"setup_manipulation_colab.py\")\n",
    "    from setup_manipulation_colab import setup_manipulation\n",
    "    setup_manipulation(manipulation_sha='dc0eb8aa32e271d9b5299a84435694d0565346af', drake_version='20200909', drake_build='nightly')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "xyZz8R16E9jZ"
   },
   "outputs": [],
   "source": [
    "# python libraries\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt, mpld3\n",
    "from IPython.display import HTML, display\n",
    "\n",
    "# Install pyngrok.\n",
    "server_args = []\n",
    "if 'google.colab' in sys.modules:\n",
    "  server_args = ['--ngrok_http_tunnel']\n",
    "\n",
    "# Start a single meshcat server instance to use for the remainder of this notebook.\n",
    "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
    "proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)\n",
    "\n",
    "# Let's do all of our imports here, too.\n",
    "import numpy as np\n",
    "from pydrake.all import (Quaternion, RigidTransform, \n",
    "    RollPitchYaw, RotationMatrix\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "EvOQi_xQE9kY"
   },
   "source": [
    "# Problem Description\n",
    "In the lecture, we learned the basics of spatial transformations. In this exercise, you will compute simple rigid transforms applying the rules you have learned in class.\n",
    "\n",
    "**These are the main steps of the exercise:**\n",
    "1. Compute rigid transforms of frames in various reference frames.\n",
    "2. Design grasp pose using spatial transformation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "L-1Ad6xYQ2aK"
   },
   "source": [
    "# Exercise on Rigid Transforms\n",
    "\n",
    "As a brief review, we have covered two rules of spatial transformation in [class](http://manipulation.csail.mit.edu/pick.html#section3).\n",
    "\\begin{equation}{^AX^B} {^BX^C} =\n",
    "      {^AX^C},\\end{equation}\n",
    "\\begin{equation}[^AX^B]^{-1} =\n",
    "      {^BX^A}.\\end{equation}\n",
    "      \n",
    "Note that the rules of transforms are based on rules of transforming positions and rotations listed below. \n",
    "\n",
    "Addition of positions in the same frame:\n",
    "\\begin{equation}^Ap^B_F + ^Bp^C_F\n",
    "      = ^Ap^C_F.\\end{equation}\n",
    "\n",
    "The additive inverse:\n",
    "\\begin{equation}^Ap^B_F = - ^Bp^A_F.\\end{equation}\n",
    "\n",
    "Rotation of a point:\n",
    "\\begin{equation}^Ap^B_G = {^GR^F} ^Ap^B_F.\\end{equation}\n",
    "\n",
    "Chaining rotations:\n",
    "\\begin{equation}{^AR^B} {^BR^C} = {^AR^C}.\\end{equation}\n",
    "\n",
    "Inverse of rotations:\n",
    "\\begin{equation}[^AR^B]^{-1} =\n",
    "      {^BR^A}.\\end{equation}\n",
    "      \n",
    "Applying these rules will yield the same result as the ones computed by the former two rules.\n",
    "\n",
    "In Drake, you can multiply frames by \n",
    "```python\n",
    "X_AB.multiply(X_BC)\n",
    "```\n",
    "\n",
    "You may also inverse a rigid transform by the [inverse](https://drake.mit.edu/pydrake/pydrake.math.html?highlight=rigidtransform#pydrake.math.RigidTransform_.RigidTransform_[float].inverse) method.\n",
    "\n",
    "```python\n",
    "X_AB.inverse()\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "Ceqhp3vWZpzx"
   },
   "source": [
    "Now suppose you have 4 frames, namely, the world frame, frame A, frame B, and frame C defined as below.\n",
    "\n",
    "-- frame A expressed in the world frame (`X_WA`)\n",
    "\n",
    "-- frame B expressed in frame A (`X_AB`)\n",
    "\n",
    "-- frame B expressed in frame C (`X_CB`)\n",
    "\n",
    "**Calcuate the following transforms by filling your code below in the designated functions.**\n",
    "\n",
    "(1) `X_WB`, frame B expressed in the world frame\n",
    "\n",
    "(2) `X_CW`, the world frame expressed in frame C"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "FDPul5bRZpzy"
   },
   "outputs": [],
   "source": [
    "def compute_X_WB(X_WA, X_AB, X_CB):\n",
    "    \"\"\"\n",
    "    fill your code here\n",
    "    \"\"\"\n",
    "    X_WB = RigidTransform()\n",
    "    return X_WB"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "xJQ1vThXZpz6"
   },
   "outputs": [],
   "source": [
    "def compute_X_CW(X_WA, X_AB, X_CB):\n",
    "    \"\"\"\n",
    "    fill your code here\n",
    "    \"\"\"\n",
    "    X_CW = RigidTransform()\n",
    "    return X_CW"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "3DJxbaN6wjMw"
   },
   "source": [
    "# Design Grasp Pose\n",
    "The grasp pose is commonly defined in the object frame so that the grasp pose ${^OX^G}$ is independent of the pose of the object. The grasp pose in the world frame can be computed by \n",
    "\n",
    "\\begin{equation}{{^WX^G} = {}{^W}X^{O}} {^OX^G},\\end{equation}\n",
    "\n",
    "where $W$ stands for the world frame and $G$ denotes the grasp frame, following the convention in the textbook. \n",
    "\n",
    "You should notice from the visualization below that the gripper frame is different from the world frame. In particular, the +y axis of the gripper frame points vertically downward, and the +z axis of the gripper points backward. This is an important observation for this exercise."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "o-5gZ-jPJJ-9"
   },
   "source": [
    "**Now for your exercise, dsign a grasp pose that satisfy the conditions below**\n",
    "\n",
    "- **gripper's position should be 0.02 unit distance above the target object in the world frame**\n",
    "- **gripper's y axis should align with object's x axis**\n",
    "- **gripper's x axis should align with object's z axis**\n",
    "- **write grasp pose in the object frame and the world frame**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "txN1Mlk5MTjl"
   },
   "source": [
    "<img src=\"https://raw.githubusercontent.com/RussTedrake/manipulation/master/figures/exercises/grasp_pose_design.png\" width=\"500\">\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "ju-etoLzJ5zz"
   },
   "outputs": [],
   "source": [
    "p0_WO = [-0.2, -0.65, 0.12] # object in world frame\n",
    "R0_WO = RotationMatrix.MakeYRotation(np.pi/2)\n",
    "X_WO = RigidTransform(R0_WO, p0_WO)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "84Po2q-MOLcJ"
   },
   "outputs": [],
   "source": [
    "def design_grasp_pose(X_WO):\n",
    "  \"\"\"\n",
    "  fill in our code below\n",
    "  \"\"\"\n",
    "  X_OG = RigidTransform()\n",
    "  X_WG = RigidTransform()\n",
    "  return X_OG, X_WG"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "zPmeRLtJk410"
   },
   "source": [
    "## How will this notebook be Graded?##\n",
    "\n",
    "If you are enrolled in the class, this notebook will be graded using [Gradescope](www.gradescope.com). You should have gotten the enrollement code on our announcement in Piazza. \n",
    "\n",
    "For submission of this assignment, you must do two things. \n",
    "- Download and submit the notebook `rigid_transforms.ipynb` to Gradescope's notebook submission section, along with your notebook for the other problems.\n",
    "\n",
    "We will evaluate the local functions in the notebook to see if the function behaves as we have expected. For this exercise, the rubric is as follows:\n",
    "- [1 pts] `compute_X_WB` is correct\n",
    "- [1 pts] `compute_X_CW` is correct\n",
    "- [2 pts] `design_grasp_pose` is correct according to the requirement"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "T2PwWuqT_O3r"
   },
   "outputs": [],
   "source": [
    "from manipulation.exercises.pick.test_rigid_transforms import TestRigidTransforms\n",
    "from manipulation.exercises.grader import Grader \n",
    "\n",
    "Grader.grade_output([TestRigidTransforms], [locals()], 'results.json')\n",
    "Grader.print_test_results('results.json')"
   ]
  }
 ],
 "metadata": {
  "celltoolbar": "Tags",
  "colab": {
   "collapsed_sections": [],
   "name": "rigid_transforms.ipynb",
   "provenance": []
  },
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}